#!/usr/bin/env python3
# -*-coding:utf8-*-

import time
import threading
from piper_sdk import *
from piper_sdk.interface.piper_interface_v2 import C_PiperInterface_V2
import sys
import os

# 添加项目根目录到路径，确保可以导入根目录下的config模块
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from config.config import *

class PiperRobot:
    def __init__(self):
        """
        初始化机器人控制器
        """
        self.piper = None
        try:
            # 初始化piper_sdk接口，使用config中定义的CAN变量
            self.piper = C_PiperInterface_V2(CAN)
            self.piper.ConnectPort()
            print("✅ 机械臂连接成功！")
            # 等待机械臂使能
            while not self.piper.EnablePiper():
                time.sleep(0.01)
            print("✅ 机械臂已使能！")
        except Exception as e:
            print(f"❌ 机械臂初始化失败: {e}")

        # 任务控制变量
        self.current_task = None
        self.stop_task_flag = threading.Event()
        self.task_lock = threading.Lock()

    def back_zero(self):
        """
        机械臂回到零点位置
        """
        try:
            if not self.piper:
                return False
                
            print("🔄 机械臂正在回到零点位置...")
            # 切换到关节控制模式
            self.piper.ModeCtrl(ctrl_mode=0x01, move_mode=0x01, move_spd_rate_ctrl=30)
            time.sleep(0.1)
            
            # 将所有关节回零
            self.piper.JointCtrl(0, 0, 0, 0, 0, 0)
            time.sleep(3)  # 给予足够的时间让机械臂移动
            print("✅ 机械臂已回到零点位置")
            return True
        except Exception as e:
            print(f"❌ 回零点失败: {e}")
            return False

    def move_to_c(self):
        """
        机械臂执行C型轨迹动作
        """
        try:
            if not self.piper:
                return False
                
            print("🔄 机械臂正在执行C型轨迹...")
            # 夹爪控制
            self.piper.GripperCtrl(0, 1000, 0x01, 0)
            
            # 设置第一个点位
            self.piper.EndPoseCtrl(135481, 9349, 161129, 178756, 6035, -178440)
            self.piper.MoveCAxisUpdateCtrl(0x01)
            time.sleep(0.001)
            
            # 设置第二个点位
            self.piper.EndPoseCtrl(222158, 128758, 142126, 175152, -1259, -157235)
            self.piper.MoveCAxisUpdateCtrl(0x02)
            time.sleep(0.001)
            
            # 设置第三个点位
            self.piper.EndPoseCtrl(359079, 3221, 153470, 179038, 1105, 179035)
            self.piper.MoveCAxisUpdateCtrl(0x03)
            time.sleep(0.001)
            
            # 执行C型轨迹
            self.piper.MotionCtrl_2(0x01, 0x03, 30, 0x00)
            print("✅ C型轨迹执行完成")
            return True
        except Exception as e:
            print(f"❌ C型轨迹执行失败: {e}")
            return False

    def stop_current_task(self):
        """
        停止当前任务
        """
        self.stop_task_flag.set()
        if self.current_task and self.current_task.is_alive():
            self.current_task.join(timeout=1.0)
        self.stop_task_flag.clear()

    def cleanup(self):
        """
        清理资源
        """
        self.stop_current_task()
        if self.piper:
            try:
                self.piper.DisconnectPort()
                print("✅ 机械臂已断开连接")
            except Exception as e:
                print(f"❌ 机械臂断开连接失败: {e}") 